#!/usr/bin/env python

import rospy
import random
from sensor_msgs.msg import NavSatFix
from std_msgs.msg import Header

def gps_simulator():
    # 初始化ROS节点
    rospy.init_node('gps_simulator', anonymous=True)

    # 创建发布者，发布到 /mavros/global_position/global 话题
    # pub = rospy.Publisher('/mavros/global_position/global', NavSatFix, queue_size=10)
    pub = rospy.Publisher('my_gps/fix', NavSatFix, queue_size=10)

    # 设置发布频率 (5Hz)
    rate = rospy.Rate(5)

    while not rospy.is_shutdown():
        # 创建模拟的GPS数据
        gps_msg = NavSatFix()

        # 设置消息头
        gps_msg.header = Header()
        gps_msg.header.stamp = rospy.Time.now()  # 当前时间作为时间戳
        gps_msg.header.frame_id = "gps"

        # 生成随机的经度、纬度和海拔
        # gps_msg.latitude = 21.679370 + random.uniform(-0.00008, 0.00008)  # 模拟纬度 (旧金山附近)
        # gps_msg.longitude = 110.919167 + random.uniform(-0.00008, 0.00008)  # 模拟经度 (旧金山附近)
        # gps_msg.altitude = 55.643506 + random.uniform(-1, 1)  # 模拟海拔
        gps_msg.latitude = 21.679370  # 模拟纬度
        gps_msg.longitude = 110.919167  # 模拟经度 
        gps_msg.altitude = 55.643506  # 模拟海拔

        # 设置位置协方差类型 (0 表示未知)
        gps_msg.position_covariance_type = 0

        # 发布模拟的GPS数据
        pub.publish(gps_msg)
        rospy.loginfo(f"Published simulated GPS data: {gps_msg}")

        # 控制发布频率
        rate.sleep()

if __name__ == '__main__':
    try:
        gps_simulator()
    except rospy.ROSInterruptException:
        pass
